#include "mapa.h"
#include "algoritmos.h"
#include <vector>
#include <limits>
#include <iostream>
#include <set>
#include <cmath>

using namespace std;

vector<float> distancias;
vector<float> heuristica_eucl;
vector<float> heuristica_manh;
vector<float> heuristica_heu3;
vector<float> heuristica_heu4;

struct cmp_dijkstra
{
    bool operator() (const int &a, const int &b)
    {
        if (distancias[a] < distancias[b]) return true;
        if (distancias[a] > distancias[b]) return false;
        return a<b;
    }
};

struct cmp_astar_eucl
{
    bool operator() (const int &a, const int &b)
    {
        if (distancias[a]+heuristica_eucl[a] < distancias[b]+heuristica_eucl[b]) return true;
        if (distancias[a]+heuristica_eucl[a] > distancias[b]+heuristica_eucl[b]) return false;
        return a<b;
    }
};

struct cmp_astar_manh
{
    bool operator() (const int &a, const int &b)
    {
        if (distancias[a]+heuristica_manh[a] < distancias[b]+heuristica_manh[b]) return true;
        if (distancias[a]+heuristica_manh[a] > distancias[b]+heuristica_manh[b]) return false;
        return a<b;
    }
};

struct cmp_astar_heu3
{
    bool operator() (const int &a, const int &b)
    {
        if (distancias[a]+heuristica_heu3[a] < distancias[b]+heuristica_heu3[b]) return true;
        if (distancias[a]+heuristica_heu3[a] > distancias[b]+heuristica_heu3[b]) return false;
        return a<b;
    }
};

struct cmp_astar_heu4
{
    bool operator() (const int &a, const int &b)
    {
        if (distancias[a]+heuristica_heu4[a] < distancias[b]+heuristica_heu4[b]) return true;
        if (distancias[a]+heuristica_heu4[a] > distancias[b]+heuristica_heu4[b]) return false;
        return a<b;
    }
};


set<unsigned int, cmp_dijkstra> Q;
set<unsigned int, cmp_astar_eucl> Lista_Abierta_E;
set<unsigned int, cmp_astar_manh> Lista_Abierta_M;
set<unsigned int, cmp_astar_heu3> Lista_Abierta_H3;
set<unsigned int, cmp_astar_heu4> Lista_Abierta_H4;

set<unsigned int> Lista_Cerrada;

void resolverDijkstra(Mapa* M, casillero inicio, casillero destino){
	bool alcanzado = false;
	unsigned int w = M->Ancho();
	unsigned int h = M->Alto();
	unsigned int SIZE = w*h;
	
	set<unsigned int>::iterator it;
	list<unsigned int>::iterator itl;	

	// Coordenadas escritas como si la matriz fuera un vector
	unsigned int poslineal_ini = inicio.x * w + inicio.y;
	unsigned int poslineal_dest = destino.x * w + destino.y;
	
	unsigned int u=SIZE, v;

	// Inicializa todas las distancias en Infinito
	distancias.clear();
	distancias.resize(w*h, INFINITO);
	
	
	// Heap casero
	Q.clear();

	// Lista de predecesores
	vector<unsigned int>* predecesores = new vector<unsigned int> (h*w, SIZE);
	vector<vector<unsigned char> >* recorrido = new vector< vector<unsigned char> > (h, vector<unsigned char>(w,NO_EXPLORADO) );

	// Distancia al inicio;
	distancias[poslineal_ini] = 0;
	(*predecesores)[poslineal_ini] = poslineal_ini; 
	(*recorrido)[inicio.x][inicio.y] = SOLUCION;
	
	// Listas de dyacentes a un nodo
	list<unsigned int> n_adyacentes;

	
	// Agrego todas los nodos a Q
	for(unsigned int i = 0; i < SIZE; i++)
		Q.insert(i);

    while( !Q.empty() )
    {
        // Tomo el siguiente nodo
        u = *(Q.begin());
        Q.erase(Q.begin());
        
        if ((*M).direcciones(u / (*M).Ancho(),u % (*M).Ancho()) == FULL_RESTRIC)
			break;

		if (u == poslineal_dest)
			alcanzado = true;

		else {
			n_adyacentes = (*M).adyacentes(u);
			for(itl = n_adyacentes.begin(); itl != n_adyacentes.end(); itl++) {
				if (distancias[*itl] > distancias[u] + (*M).tiempo(u)) {
					//Actualizo las distancias
					it = Q.find(*itl);
					v = *it;
					
					Q.erase(it);					
					distancias[*itl] = distancias[u] + (*M).tiempo(u);				
					Q.insert(v);
					
					(*recorrido)[*itl / (*M).Ancho()][*itl % (*M).Ancho()] = VISITADO;
					(*predecesores)[*itl] = u;
				}
			}
        }
    }
    
    if (alcanzado){
		v = poslineal_dest;
		while ((*predecesores)[v] != v) {
			(*recorrido)[v / (*M).Ancho()][v % (*M).Ancho()] = SOLUCION;
			v = (*predecesores)[v];
		}
	}    
		
	mostrarSolucion(recorrido);
	
	delete predecesores;
	delete recorrido;
	
};





void resolverAStar_Eucl(Mapa* M, casillero inicio, casillero destino){
	bool alcanzado = false;
	unsigned int w = M->Ancho();
	unsigned int h = M->Alto();
	unsigned int SIZE = w*h;

	set<unsigned int>::iterator it;
	list<unsigned int>::iterator itl;	

	// Coordenadas escritas como si la matriz fuera un vector
	unsigned int poslineal_ini = inicio.x * w + inicio.y;
	unsigned int poslineal_dest = destino.x * w + destino.y;
	
	unsigned int u, v;

	// Inicializa todas las distancias en Infinito
	distancias.clear();
	distancias.resize(w*h, INFINITO);
	
	// Inicializa las heuristicas
	heuristica_eucl.clear();
	heuristica_eucl.resize(w*h, 0);
	for(unsigned int i=0; i<SIZE; i++) {
		float x = i / (*M).Ancho(); 
		float y = i % (*M).Ancho();
		heuristica_eucl[i] = sqrtf( (x-destino.x)*(x-destino.x) + (y-destino.y)*(y-destino.y) );
	}
	
	
	// Heap casero
	Lista_Abierta_E.clear();
	Lista_Cerrada.clear();
	

	// Lista de predecesores
	vector<unsigned int>* predecesores = new vector<unsigned int> (h*w, SIZE);
	vector<vector<unsigned char> >* recorrido = new vector< vector<unsigned char> > (h, vector<unsigned char>(w,NO_EXPLORADO) );

	// Distancia al inicio;
	distancias[poslineal_ini] = 0;
	(*predecesores)[poslineal_ini] = poslineal_ini; 
	(*recorrido)[inicio.x][inicio.y] = SOLUCION;
	
	// Listas de dyacentes a un nodo
	list<unsigned int> n_adyacentes;

	
	// Agrego el nodo inicial
	Lista_Abierta_E.insert(poslineal_ini);

    while( !alcanzado && !Lista_Abierta_E.empty())
    {
        // Tomo el siguiente nodo
        u = *(Lista_Abierta_E.begin());
        Lista_Abierta_E.erase(Lista_Abierta_E.begin());
        Lista_Cerrada.insert(u);
        
        // Si el nodo es intransitable o tiene distancia infinita, no hay solución
        if ((*M).direcciones(u / (*M).Ancho(),u % (*M).Ancho()) == FULL_RESTRIC)
			break;
		if (distancias[u] == INFINITO)
			break;			

		// Si llegó, cambia la guarda
		if (u == poslineal_dest)
			alcanzado = true;

		else {
			n_adyacentes = (*M).adyacentes(u);
			for(itl = n_adyacentes.begin(); itl != n_adyacentes.end(); itl++) {
									
				// Ignoramos aquellos que son instransitables o están en la lista cerrada
				if ((*M).direcciones(*itl / (*M).Ancho(),*itl % (*M).Ancho()) == FULL_RESTRIC)
					continue;
				if (Lista_Cerrada.find(*itl) != Lista_Cerrada.end() )
					continue;

				// Si está en la lista abierta, actualizamos distancias de ser necesario
				if ( (it = Lista_Abierta_E.find(*itl)) != Lista_Abierta_E.end()){
					if (distancias[*itl] > distancias[u] + (*M).tiempo(u)) {
						
						Lista_Abierta_E.erase(it);
						distancias[*itl] = distancias[u] + (*M).tiempo(u);
						Lista_Abierta_E.insert(*itl);
					
						(*recorrido)[*itl / (*M).Ancho()][*itl % (*M).Ancho()] = VISITADO;
						(*predecesores)[*itl] = u;
					}
				}
				else {

					if  (*itl == poslineal_dest)
						alcanzado = true;
					distancias[*itl] = distancias[u] + (*M).tiempo(u);
					(*recorrido)[*itl / (*M).Ancho()][*itl % (*M).Ancho()] = VISITADO;
					(*predecesores)[*itl] = u;
					Lista_Abierta_E.insert(*itl);

				}
			}
        }
    }
    
    if (alcanzado){
		v = poslineal_dest;
		while ((*predecesores)[v] != v) {
			(*recorrido)[v / (*M).Ancho()][v % (*M).Ancho()] = SOLUCION;
			v = (*predecesores)[v];
		}
	}    
		
	mostrarSolucion(recorrido);
	
	delete predecesores;
	delete recorrido;


};




void resolverAStar_Manh(Mapa* M, casillero inicio, casillero destino){
	bool alcanzado = false;
	unsigned int w = M->Ancho();
	unsigned int h = M->Alto();
	unsigned int SIZE = w*h;

	set<unsigned int>::iterator it;
	list<unsigned int>::iterator itl;	

	// Coordenadas escritas como si la matriz fuera un vector
	unsigned int poslineal_ini = inicio.x * w + inicio.y;
	unsigned int poslineal_dest = destino.x * w + destino.y;
	
	unsigned int u, v;

	// Inicializa todas las distancias en Infinito
	distancias.clear();
	distancias.resize(w*h, INFINITO);
	
	// Inicializa las heuristicas
	heuristica_manh.clear();
	heuristica_manh.resize(w*h, 0);
	for(unsigned int i=0; i<SIZE; i++) {
		float x = i / (*M).Ancho(); 
		float y = i % (*M).Ancho();
		heuristica_manh[i] = abs(x-destino.x) + abs (y-destino.y);
	}
	
	
	// Heap casero
	Lista_Abierta_M.clear();
	Lista_Cerrada.clear();
	

	// Lista de predecesores
	vector<unsigned int>* predecesores = new vector<unsigned int> (h*w, SIZE);
	vector<vector<unsigned char> >* recorrido = new vector< vector<unsigned char> > (h, vector<unsigned char>(w,NO_EXPLORADO) );

	// Distancia al inicio;
	distancias[poslineal_ini] = 0;
	(*predecesores)[poslineal_ini] = poslineal_ini; 
	(*recorrido)[inicio.x][inicio.y] = SOLUCION;
	
	// Listas de dyacentes a un nodo
	list<unsigned int> n_adyacentes;

	
	// Agrego el nodo inicial
	Lista_Abierta_M.insert(poslineal_ini);

    while( !alcanzado && !Lista_Abierta_M.empty())
    {
        // Tomo el siguiente nodo
        u = *(Lista_Abierta_M.begin());
        Lista_Abierta_M.erase(Lista_Abierta_M.begin());
        Lista_Cerrada.insert(u);
        
        // Si el nodo es intransitable o tiene distancia infinita, no hay solución
        if ((*M).direcciones(u / (*M).Ancho(),u % (*M).Ancho()) == FULL_RESTRIC)
			break;
		if (distancias[u] == INFINITO)
			break;			

		// Si llegó, cambia la guarda
		if (u == poslineal_dest)
			alcanzado = true;

		else {
			n_adyacentes = (*M).adyacentes(u);
			for(itl = n_adyacentes.begin(); itl != n_adyacentes.end(); itl++) {
									
				// Ignoramos aquellos que son instransitables o están en la lista cerrada
				if ((*M).direcciones(*itl / (*M).Ancho(),*itl % (*M).Ancho()) == FULL_RESTRIC)
					continue;
				if (Lista_Cerrada.find(*itl) != Lista_Cerrada.end() )
					continue;

				// Si está en la lista abierta, actualizamos distancias de ser necesario
				if ( (it = Lista_Abierta_M.find(*itl)) != Lista_Abierta_M.end()){
					if (distancias[*itl] > distancias[u] + (*M).tiempo(u)) {
						
						Lista_Abierta_M.erase(it);
						distancias[*itl] = distancias[u] + (*M).tiempo(u);
						Lista_Abierta_M.insert(*itl);
					
						(*recorrido)[*itl / (*M).Ancho()][*itl % (*M).Ancho()] = VISITADO;
						(*predecesores)[*itl] = u;
					}
				}
				else {

					if  (*itl == poslineal_dest)
						alcanzado = true;
					distancias[*itl] = distancias[u] + (*M).tiempo(u);
					(*recorrido)[*itl / (*M).Ancho()][*itl % (*M).Ancho()] = VISITADO;
					(*predecesores)[*itl] = u;
					Lista_Abierta_M.insert(*itl);

				}
			}
        }
    }
    
    if (alcanzado){
		v = poslineal_dest;
		while ((*predecesores)[v] != v) {
			(*recorrido)[v / (*M).Ancho()][v % (*M).Ancho()] = SOLUCION;
			v = (*predecesores)[v];
		}
	}    
		
	mostrarSolucion(recorrido);
	
	delete predecesores;
	delete recorrido;
	
};

void resolverAStar_Heur3(Mapa* M, casillero inicio, casillero destino){
	bool alcanzado = false;
	unsigned int w = M->Ancho();
	unsigned int h = M->Alto();
	unsigned int SIZE = w*h;

	set<unsigned int>::iterator it;
	list<unsigned int>::iterator itl;	

	// Coordenadas escritas como si la matriz fuera un vector
	unsigned int poslineal_ini = inicio.x * w + inicio.y;
	unsigned int poslineal_dest = destino.x * w + destino.y;
	
	unsigned int u, v;

	// Inicializa todas las distancias en Infinito
	distancias.clear();
	distancias.resize(w*h, INFINITO);
	
	// Inicializa las heuristicas
	heuristica_heu3.clear();
	heuristica_heu3.resize(w*h, 0);
	for(unsigned int i=0; i<SIZE; i++) {
		float x = i / (*M).Ancho(); 
		float y = i % (*M).Ancho();
		heuristica_heu3[i] = abs( (x-destino.x)*(inicio.y - destino.y) - (y-destino.y)*(inicio.x-destino.x) );
	}
	
	
	// Heap casero
	Lista_Abierta_H3.clear();
	Lista_Cerrada.clear();
	

	// Lista de predecesores
	vector<unsigned int>* predecesores = new vector<unsigned int> (h*w, SIZE);
	vector<vector<unsigned char> >* recorrido = new vector< vector<unsigned char> > (h, vector<unsigned char>(w,NO_EXPLORADO) );

	// Distancia al inicio;
	distancias[poslineal_ini] = 0;
	(*predecesores)[poslineal_ini] = poslineal_ini; 
	(*recorrido)[inicio.x][inicio.y] = SOLUCION;
	
	// Listas de dyacentes a un nodo
	list<unsigned int> n_adyacentes;

	
	// Agrego el nodo inicial
	Lista_Abierta_H3.insert(poslineal_ini);

    while( !alcanzado && !Lista_Abierta_H3.empty())
    {
        // Tomo el siguiente nodo
        u = *(Lista_Abierta_H3.begin());
        Lista_Abierta_H3.erase(Lista_Abierta_H3.begin());
        Lista_Cerrada.insert(u);
        
        // Si el nodo es intransitable o tiene distancia infinita, no hay solución
        if ((*M).direcciones(u / (*M).Ancho(),u % (*M).Ancho()) == FULL_RESTRIC)
			break;
		if (distancias[u] == INFINITO)
			break;			

		// Si llegó, cambia la guarda
		if (u == poslineal_dest)
			alcanzado = true;

		else {
			n_adyacentes = (*M).adyacentes(u);
			for(itl = n_adyacentes.begin(); itl != n_adyacentes.end(); itl++) {
									
				// Ignoramos aquellos que son instransitables o están en la lista cerrada
				if ((*M).direcciones(*itl / (*M).Ancho(),*itl % (*M).Ancho()) == FULL_RESTRIC)
					continue;
				if (Lista_Cerrada.find(*itl) != Lista_Cerrada.end() )
					continue;

				// Si está en la lista abierta, actualizamos distancias de ser necesario
				if ( (it = Lista_Abierta_H3.find(*itl)) != Lista_Abierta_H3.end()){
					if (distancias[*itl] > distancias[u] + (*M).tiempo(u)) {
						
						Lista_Abierta_H3.erase(it);
						distancias[*itl] = distancias[u] + (*M).tiempo(u);
						Lista_Abierta_H3.insert(*itl);
					
						(*recorrido)[*itl / (*M).Ancho()][*itl % (*M).Ancho()] = VISITADO;
						(*predecesores)[*itl] = u;
					}
				}
				else {

					if  (*itl == poslineal_dest)
						alcanzado = true;
					distancias[*itl] = distancias[u] + (*M).tiempo(u);
					(*recorrido)[*itl / (*M).Ancho()][*itl % (*M).Ancho()] = VISITADO;
					(*predecesores)[*itl] = u;
					Lista_Abierta_H3.insert(*itl);

				}
			}
        }
    }
    
    if (alcanzado){
		v = poslineal_dest;
		while ((*predecesores)[v] != v) {
			(*recorrido)[v / (*M).Ancho()][v % (*M).Ancho()] = SOLUCION;
			v = (*predecesores)[v];
		}
	}    
		
	mostrarSolucion(recorrido);
	
	delete predecesores;
	delete recorrido;

};

void resolverAStar_Heur4(Mapa* M, casillero inicio, casillero destino){
	bool alcanzado = false;
	unsigned int w = M->Ancho();
	unsigned int h = M->Alto();
	unsigned int SIZE = w*h;

	set<unsigned int>::iterator it;
	list<unsigned int>::iterator itl;	

	// Coordenadas escritas como si la matriz fuera un vector
	unsigned int poslineal_ini = inicio.x * w + inicio.y;
	unsigned int poslineal_dest = destino.x * w + destino.y;
	
	unsigned int u, v;

	// Inicializa todas las distancias en Infinito
	distancias.clear();
	distancias.resize(w*h, INFINITO);
	
	// Inicializa las heuristicas
	heuristica_heu4.clear();
	heuristica_heu4.resize(w*h, 0);
	for(unsigned int i=0; i<SIZE; i++) {
		float x = i / (*M).Ancho(); 
		float y = i % (*M).Ancho();
		heuristica_heu4[i] = (abs(x-destino.x) + abs (y-destino.y));
		heuristica_heu4[i] = heuristica_heu4[i] * 1.001;
	}
	
	
	// Heap casero
	Lista_Abierta_H4.clear();
	Lista_Cerrada.clear();
	

	// Lista de predecesores
	vector<unsigned int>* predecesores = new vector<unsigned int> (h*w, SIZE);
	vector<vector<unsigned char> >* recorrido = new vector< vector<unsigned char> > (h, vector<unsigned char>(w,NO_EXPLORADO) );

	// Distancia al inicio;
	distancias[poslineal_ini] = 0;
	(*predecesores)[poslineal_ini] = poslineal_ini; 
	(*recorrido)[inicio.x][inicio.y] = SOLUCION;
	
	// Listas de dyacentes a un nodo
	list<unsigned int> n_adyacentes;

	
	// Agrego el nodo inicial
	Lista_Abierta_H4.insert(poslineal_ini);

    while( !alcanzado && !Lista_Abierta_H4.empty())
    {
        // Tomo el siguiente nodo
        u = *(Lista_Abierta_H4.begin());
        Lista_Abierta_H4.erase(Lista_Abierta_H4.begin());
        Lista_Cerrada.insert(u);
        
        // Si el nodo es intransitable o tiene distancia infinita, no hay solución
        if ((*M).direcciones(u / (*M).Ancho(),u % (*M).Ancho()) == FULL_RESTRIC)
			break;
		if (distancias[u] == INFINITO)
			break;			

		// Si llegó, cambia la guarda
		if (u == poslineal_dest)
			alcanzado = true;

		else {
			n_adyacentes = (*M).adyacentes(u);
			for(itl = n_adyacentes.begin(); itl != n_adyacentes.end(); itl++) {
									
				// Ignoramos aquellos que son instransitables o están en la lista cerrada
				if ((*M).direcciones(*itl / (*M).Ancho(),*itl % (*M).Ancho()) == FULL_RESTRIC)
					continue;
				if (Lista_Cerrada.find(*itl) != Lista_Cerrada.end() )
					continue;

				// Si está en la lista abierta, actualizamos distancias de ser necesario
				if ( (it = Lista_Abierta_H4.find(*itl)) != Lista_Abierta_H4.end()){
					if (distancias[*itl] > distancias[u] + (*M).tiempo(u)) {
						
						Lista_Abierta_H4.erase(it);
						distancias[*itl] = distancias[u] + (*M).tiempo(u);
						Lista_Abierta_H4.insert(*itl);
					
						(*recorrido)[*itl / (*M).Ancho()][*itl % (*M).Ancho()] = VISITADO;
						(*predecesores)[*itl] = u;
					}
				}
				else {

					if  (*itl == poslineal_dest)
						alcanzado = true;
					distancias[*itl] = distancias[u] + (*M).tiempo(u);
					(*recorrido)[*itl / (*M).Ancho()][*itl % (*M).Ancho()] = VISITADO;
					(*predecesores)[*itl] = u;
					Lista_Abierta_H4.insert(*itl);

				}
			}
        }
    }
    
    if (alcanzado){
		v = poslineal_dest;
		while ((*predecesores)[v] != v) {
			(*recorrido)[v / (*M).Ancho()][v % (*M).Ancho()] = SOLUCION;
			v = (*predecesores)[v];
		}
	}    
		
	mostrarSolucion(recorrido);
	
	delete predecesores;
	delete recorrido;
	
};


void resolver(Mapa* M, unsigned int algoritmo, casillero inicio, casillero destino)
{
	switch(algoritmo) {
		case DIJKSTRA:
						resolverDijkstra(M, inicio, destino);
						break;
		case ASTAR_EUCLID:
						resolverAStar_Eucl(M, inicio, destino);
						break;
		case ASTAR_MANH:
						resolverAStar_Manh(M, inicio, destino);
						break;
		case ASTAR_HEUR3:
						resolverAStar_Heur3(M, inicio, destino);
						break;
		case ASTAR_HEUR4:
						resolverAStar_Heur4(M, inicio, destino);
						break;		
		default:	resolverDijkstra(M, inicio, destino);
	}
	
}



void mostrarSolucion(const vector<vector<unsigned char> >* recorrido) {
	unsigned int w = ((*recorrido)[0]).size();
	for(unsigned int i=0; i < (*recorrido).size(); i++) {
		for(unsigned int j=0; j < w; j++)
			cout << (*recorrido)[i][j];
		cout << endl;
	}
}

